#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <tf2_ros/transform_listener.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <mutex>

class LaserScanToPC
{
public:
    LaserScanToPC(int n)
        : n_(n),
          point_clouds_(n),
          pc_pub_(nh_.advertise<sensor_msgs::PointCloud2>("/lidar_scan", 1)),
          tf_buffer_(),
          tf_listener_(tf_buffer_)
    {
        for (int i = 0; i < n_; ++i)
        {
            std::string topic = "/scan_" + std::to_string(i);
            // bind to a class member function using boost::function
            scan_subs_.push_back(
                nh_.subscribe<sensor_msgs::LaserScan>(
                    topic, 1,
                    boost::function<void(const sensor_msgs::LaserScan::ConstPtr &)>(
                        boost::bind(&LaserScanToPC::scanCallback, this, i, _1)),
                    ros::VoidPtr(),
                    ros::TransportHints().tcpNoDelay()));
        }
    }

    void scanCallback(int index, const sensor_msgs::LaserScan::ConstPtr &scan)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        double angle = scan->angle_min;
        for (size_t i = 0; i < scan->ranges.size(); ++i)
        {
            float range = scan->ranges[i];
            if (range < scan->range_min || range > scan->range_max)
                continue;
            double x = range * cos(angle);
            double y = range * sin(angle);
            double z = 0.0;
            try
            {
                geometry_msgs::TransformStamped transform =
                    tf_buffer_.lookupTransform("base_link", scan->header.frame_id, ros::Time(0));
                pcl::PointXYZ pt;
                pt.x = transform.transform.translation.x + x;
                pt.y = transform.transform.translation.y + y;
                pt.z = transform.transform.translation.z + z;
                cloud->points.push_back(pt);
            }
            catch (tf2::TransformException &ex)
            {
                ROS_WARN("%s", ex.what());
                return;
            }
            angle += scan->angle_increment;
        }
        std::lock_guard<std::mutex> lock(pcl_lock_);
        point_clouds_[index] = *cloud;
    }

    void mergeAndPublish()
    {
        pcl::PointCloud<pcl::PointXYZ> merged_cloud;
        std::lock_guard<std::mutex> lock(pcl_lock_);
        for (const auto &cloud : point_clouds_)
        {
            merged_cloud += cloud;
        }

        sensor_msgs::PointCloud2 output;
        pcl::toROSMsg(merged_cloud, output);
        output.header.frame_id = "base_link";
        output.header.stamp = ros::Time::now();
        pc_pub_.publish(output);
    }

private:
    ros::NodeHandle nh_;
    ros::Publisher pc_pub_;
    std::vector<ros::Subscriber> scan_subs_;
    std::vector<pcl::PointCloud<pcl::PointXYZ>> point_clouds_;
    int n_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    std::mutex pcl_lock_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "laserscans2pc");
    int n = 30;
    LaserScanToPC converter(n);
    ros::Rate rate(20);
    while (ros::ok())
    {
        converter.mergeAndPublish();
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
